/*
 * PrimaryStateLeaveTape.c
 */

#include <p24FJ64GA002.h>
#include "pwm.h"
#include "sensors.h"
#include "tick.h"
#include "states.h"

/* ************************************************************************** */
/* internal states */
#define LEAVE_TAPE_STATE_INIT        1
#define LEAVE_TAPE_STATE_ROTATE_CW   2
#define LEAVE_TAPE_STATE_ARC_LEFT    3
#define LEAVE_TAPE_STATE_DETECT_LINE 4

/* clock tick count constants */
#define ROTATE_CW_TICK_COUNT    500
#define MOVE_FORWARD_TICK_COUNT 4000

/* rotate constants */
#ifdef OLD_CONSTANTS
    #define ROTATE_RISING_EDGE_TIME             1750
    #define OC1R_FIRST_STAGE_RISING_EDGE_TIME   1900
    #define OC2R_FIRST_STAGE_RISING_EDGE_TIME   2350
    #define OC1R_SECOND_STAGE_RISING_EDGE_TIME  2100
    #define OC2R_SECOND_STAGE_RISING_EDGE_TIME  2350
#else
    #define ROTATE_RISING_EDGE_TIME             1750
    #define OC1R_FIRST_STAGE_RISING_EDGE_TIME   900
    #define OC2R_FIRST_STAGE_RISING_EDGE_TIME   1350
    #define OC1R_SECOND_STAGE_RISING_EDGE_TIME  900
    #define OC2R_SECOND_STAGE_RISING_EDGE_TIME  1050
#endif

/* prototypes */
void leave_tape_init(void);
void leave_tape_rotate_cw(void);
void leave_tape_arc_left(void);
void leave_tape_detect_line(void);

/* globals */
static unsigned leave_tape_state = LEAVE_TAPE_STATE_INIT;
static unsigned compare_tick = 0;
static unsigned ticks = 0;

/* ************************************************************************** */
void PrimaryStateLeaveTape(void) {
    switch (leave_tape_state) {
        case LEAVE_TAPE_STATE_INIT:
            leave_tape_init();
            break;
        case LEAVE_TAPE_STATE_ROTATE_CW:
            leave_tape_rotate_cw();
            break;
        case LEAVE_TAPE_STATE_ARC_LEFT:
            leave_tape_arc_left();
            break;
        case LEAVE_TAPE_STATE_DETECT_LINE:
            leave_tape_detect_line();
            break;
        default:
            leave_tape_state = LEAVE_TAPE_STATE_INIT;
    }
}

/* ************************************************************************** */
void leave_tape_init() {
    compare_tick = GetTick() + ROTATE_CW_TICK_COUNT;
    OC1R = (unsigned) ROTATE_RISING_EDGE_TIME;
    PivotCW();
    leave_tape_state = LEAVE_TAPE_STATE_ROTATE_CW;
}

void leave_tape_rotate_cw() {
    ticks = GetTick();
    if (ticks > compare_tick) {
        Stop();
        OC1R = (unsigned) OC1R_FIRST_STAGE_RISING_EDGE_TIME;
        OC2R = (unsigned) OC2R_FIRST_STAGE_RISING_EDGE_TIME;
        leave_tape_state = LEAVE_TAPE_STATE_ARC_LEFT;
        compare_tick = ticks + MOVE_FORWARD_TICK_COUNT;
        Forward();
    }
}

void leave_tape_arc_left() {
    ticks = GetTick();
    if (ticks > compare_tick) {
        OC1R = (unsigned) OC1R_SECOND_STAGE_RISING_EDGE_TIME;
        OC2R = (unsigned) OC2R_SECOND_STAGE_RISING_EDGE_TIME;
        leave_tape_state = LEAVE_TAPE_STATE_DETECT_LINE;
    }
}

void leave_tape_detect_line() {
    if (IsMiddlePhotoOnTape() || IsRightPhotoOnTape() || IsLeftPhotoOnTape()) {
        SetCurrentPrimaryState(PRIMARY_STATE_FOLLOW_TAPE);
    }
}